#include "ros_worker.h"
#include "task_client_factory.h"

/*
 * ROSWorker构造函数
 * 初始化ROS节点和相关组件，设置执行器和定时器
 * @param model 用于接收心跳数据的模型对象
 * @param parent 父对象指针，默认为nullptr
 */
ROSWorker::ROSWorker(HeartbeatModel* model, QObject* parent) 
    : QObject(parent) {
    // 创建节点对象（不指定父对象）
    listener_ = std::make_shared<HeartbeatListener>(model);
    //taskClientNode_ = std::make_shared<TaskClientNode>();
    //robotStatusClient_ = std::make_shared<RobotStatusClient>();
    //mapDownloadClient_ = std::make_shared<MapDownloadActionClient>();
    
    // 创建执行器并添加所有节点
    executor_ = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
    executor_->add_node(listener_);
    //executor_->add_node(taskClientNode_);
    //executor_->add_node(robotStatusClient_);
    //executor_->add_node(mapDownloadClient_);
    
    // 配置定时器（每10ms处理ROS事件）
    connect(&workerThread_, &QThread::started, [this]() {
        spinTimer_.start(10);  // 10ms触发一次spin_some
    });
    
    connect(&spinTimer_, &QTimer::timeout, [this]() {
        executor_->spin_some();
    });
}

void ROSWorker::addTaskClientNode(TaskClientNode* client) {
    if (executor_ && client) {
        executor_->add_node(client->get_node_base_interface());
    }
}

/*
 * 启动ROS工作线程
 * 将定时器移动到工作线程并启动线程
 */
void ROSWorker::start() {
    // 将定时器移动到工作线程
    spinTimer_.moveToThread(&workerThread_);
    workerThread_.start();
}

/*
 * 停止ROS工作线程
 * 停止定时器并安全退出线程
 */
void ROSWorker::stop() {
    spinTimer_.stop();
    workerThread_.quit();
    workerThread_.wait();
}

/*
 * ROSWorker析构函数
 * 安全停止所有活动组件并释放资源
 */
ROSWorker::~ROSWorker() {
    stop();
    listener_.reset();
    //taskClientNode_.reset();
    //robotStatusClient_.reset();
    //mapDownloadClient_.reset();
    executor_.reset();
}